#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import math

ev3 = EV3Brick()
motorA = Motor(Port.A)
motorB = Motor(Port.B)
motorC = Motor(Port.C)
motorD = Motor(Port.D)

def kenetics(v_x,v_y,omega):
    omega_1 = v_x-v_y-(r_x+r_y)*omega/R
    omega_2 = v_x+v_y-(r_x+r_y)*omega/R
    omega_3 = v_x-v_y+(r_x+r_y)*omega/R
    omega_4 = v_x+v_y+(r_x+r_y)*omega/R
    output_vec = [omega_1,-omega_2,-omega_3,omega_4]
    return output_vec
# 地面坐标系
def ground_kenetics(v_x,v_y,omega,angle):
    angle = angle*math.pi/180
    omega_1 = v_x*(math.cos(angle)+math.sin(angle))+v_y*(math.sin(angle)-math.cos(angle))-(r_x+r_y)*omega/R
    omega_2 = v_x*(math.cos(angle)-math.sin(angle))+v_y*(math.sin(angle)+math.cos(angle))-(r_x+r_y)*omega/R
    omega_3 = v_x*(math.cos(angle)+math.sin(angle))+v_y*(math.sin(angle)-math.cos(angle))+(r_x+r_y)*omega/R
    omega_4 = v_x*(math.cos(angle)-math.sin(angle))+v_y*(math.sin(angle)+math.cos(angle))+(r_x+r_y)*omega/R
    output_vec = [-omega_1,omega_2,omega_3,-omega_4]
    return output_vec

kp = 3
ki = 0.01
kd = 0.01
T = 0.01
r_x = 98
r_y = 65
R = 24
x_pos_cmd = 150 #mm
omega = 2*math.pi/(5/4)

class Pid_t:
    def __init__(self,kp,ki,kd,T = 0.01):
        self.kp = kp
        self.ki = ki
        self.kd = kd

        self.ek = 0
        self.ek1 = 0
        self.ek2 = 0
        self.uk1 = 0
        self.res = 0
        self.T = T   #采样周期
    def cal(self,current,target):
        self.ek=target-current
        delta_uk=self.kp*(self.ek-self.ek1)+self.ki*self.T*self.ek+self.kd/self.T*(self.ek-2*self.ek1+self.ek2)
        self.uk1 += delta_uk
        res = self.uk1
        self.ek2=self.ek1
        self.ek1=self.ek
        return res


my_but = InfraredSensor(Port.S1)  #遥控
gyrosensor = GyroSensor(Port.S2)  #陀螺仪
mytouchsensor = TouchSensor(Port.S3)  #触碰传感器
myultra = UltrasonicSensor(Port.S4)   #超声波
mytime = StopWatch()
x_pos_pid = Pid_t(kp,ki,kd,T)
y_pos_pid = Pid_t(kp,ki,kd,T)
omega_pos_pid = Pid_t(kp,ki,kd,T)

start_time = mytime.time()
real_time = start_time
fo_speed = open("week12_kp_%.3f_ki_%.3f_kd_%.3f_xposcmd_%.3f_omega_%.3f.txt"%(kp,ki,kd,x_pos_cmd,omega),"w+")

Position_cmd = 0
Position_res = 0
start_Pos = myultra.distance()
start_Pos_times = 0
flag = False

while real_time<5000:
    real_time = mytime.time()-start_time
    if flag == False:
        if real_time<1000:
            start_Pos += myultra.distance()
            start_Pos_times += 1
        else:
            if start_Pos_times != 0:
                flag = True
                start_Pos /= start_Pos_times
                Position_cmd = x_pos_cmd + start_Pos
                print("start_Pos: %f, Position_cmd: %f"%(start_Pos,Position_cmd))
                start_Pos_times = 0
    else:
        Position_res = myultra.distance()

        v_x = x_pos_pid.cal(Position_res,Position_cmd)
        output_vec = kenetics(v_x,0,0)
        motorA.run(output_vec[0])
        motorB.run(output_vec[1]) 
        motorC.run(output_vec[2])
        motorD.run(output_vec[3])
        fo_speed.write(str(real_time)+' '+\
            str(Position_res-start_Pos)+' '+\
            str(motorA.speed())+' '+\
            str(motorB.speed())+' '+\
            str(motorC.speed())+' '+\
            str(motorD.speed())+'\n')


motorA.stop()
motorB.stop()
motorC.stop()
motorD.stop()
